
#ifndef VEHICULO_H_
#define VEHICULO_H_

#include <sigc++/signal.h>

#include <btBulletDynamicsCommon.h>
#include "fisica/objeto.h"
#include "fisica/motor.h"
#include "displaylist.h"
#include "vertexs.h"
#include "coord.h"
#include "superficiebarrido.h"
#include "bsanclada.h"
#include "bscerrada.h"
#include "bezier.h"
#include "curvacompuesta.h"
#include "discretizador.h"
#include "textura.h"

namespace {
   const float accelInc = 100;
   const float accelDec = 50;
   const float accelMax = 1500;
   const float accelMin = -500;

   const float breakInc = 50;
   const float breakMax = 500;

   const float giroInc = 0.02;
   const float giroMax = 0.5;
}

class Vehiculo : public Fisica::Objeto, /*public DisplayList,*/ public sigc::trackable {
   private:

      btDefaultVehicleRaycaster *vRayCaster;
      btRaycastVehicle *vehiculo;
      btRaycastVehicle::btVehicleTuning propiedades;

      float accel;
      float breakk;
      float giro;

      bool debeDoblarIzq, debeDoblarDer;
      bool debeAvanzar, debeRetroceder;

      /*
       * Posicionamiento respecto al centro del vehiculo sobre la cara superior.
       * Esto es, el punto (0, 0, 0) coincide con el punto (x, y, z)
       * donde x es la mitad del ancho del vehiculo,
       *       y es la mitad del largo y
       *       z es el alto del vehiculo
       * */
      void agregarRueda(float x, float y, float z, bool esDelantera, float radioRueda) {
         const float longSuspension = 0.6;

         vehiculo->addWheel(
               btVector3(x, y, z),
               btVector3(0, 0, -1), //Direccion de la suspension
               btVector3(1, 0, 0), //Direccion del eje de la rueda
               longSuspension,
               radioRueda,
               propiedades,
               esDelantera);
      }

      Vertexs *vertexSup;
      Vertexs *vertexLateral;

      Textura *sup;
      Textura *lateral;

      Vertexs *vertexLlanta;
      Vertexs *vertexRueda;

      const float anchoRueda;

   public:

      Vehiculo() : Objeto(*new btBoxShape(btVector3(1.0, 2.0, 0.2)), 23, 23, 3, 800), 
      accel(0), breakk(0), giro(0), 
      debeDoblarIzq(false), debeDoblarDer(false), 
      debeAvanzar(false), debeRetroceder(false),
      sup(0), lateral(0),
      vertexLlanta(0), vertexRueda(0),
      anchoRueda(0.05) {
         this->desactivar();

         vRayCaster = new btDefaultVehicleRaycaster(Fisica::MotorFisico::getMotor()->internal());
         vehiculo = new btRaycastVehicle(propiedades, this->cuerpoRigido(), vRayCaster);
         Fisica::MotorFisico::getMotor()->internal()->addAction(vehiculo);
         
         std::vector<Coord> coordBarrido;

         coordBarrido.push_back(Coord(-1, 0, 0.5));
         coordBarrido.push_back(Coord(-0.97, 0, 0.52));
         coordBarrido.push_back(Coord(-0.95, 0, 0.54));
         coordBarrido.push_back(Coord(-0.91, 0, 0.56));
         coordBarrido.push_back(Coord(-0.9, 0, 0.58));
         coordBarrido.push_back(Coord(-0.7, 0, 0.6));
         coordBarrido.push_back(Coord( 0, 0, 0.6));
         coordBarrido.push_back(Coord( 0.7, 0, 0.6));
         coordBarrido.push_back(Coord( 0.9, 0, 0.58));
         coordBarrido.push_back(Coord( 0.91, 0, 0.56));
         coordBarrido.push_back(Coord( 0.95, 0, 0.54));
         coordBarrido.push_back(Coord( 0.97, 0, 0.52));
         coordBarrido.push_back(Coord( 1, 0, 0.5));
         
         Bezier barrido(coordBarrido);
         

         std::vector<Coord> coordPerfil;

         coordPerfil.push_back(Coord(0,  2.0,  -0.8));
         coordPerfil.push_back(Coord(0,  1.9,  -0.0));
         coordPerfil.push_back(Coord(0,  1,    -0.15));
         coordPerfil.push_back(Coord(0,  1,    -0.15));
         coordPerfil.push_back(Coord(0,  0.5,   0.2));
         coordPerfil.push_back(Coord(0,  0,     0.2));
         coordPerfil.push_back(Coord(0, -0.4,   0.2));
         coordPerfil.push_back(Coord(0, -0.8,   0.2));
         coordPerfil.push_back(Coord(0, -1.15, -0.15));
         coordPerfil.push_back(Coord(0, -1.15, -0.15));
         coordPerfil.push_back(Coord(0, -1.9,  -0.0));
         coordPerfil.push_back(Coord(0, -2.0,  -0.8));
         
         BsplineAnclada perfil(coordPerfil);

         SuperficieBarrido sup(perfil, barrido);

         vertexSup = Discretizador(0.05, 0.1).superficie(sup);

         std::vector<Coord> coordBase;

         coordBase.push_back(Coord(0, 2.0,  -0.8));
         coordBase.push_back(Coord(0, 1.5,  -0.4));
         coordBase.push_back(Coord(0, 1.0,  -0.4));
         coordBase.push_back(Coord(0, 0.5,  -0.6));
         coordBase.push_back(Coord(0, 0.0,  -0.6));
         coordBase.push_back(Coord(0, -1.0,  -0.6));
         coordBase.push_back(Coord(0, -2.0,  -0.8));

         Bezier base(coordBase);

         vertexLateral = Discretizador(0.05).entreCurvas(perfil, base, Coord(1, 0, 0));

         /*
          *     ^  Adelante (Eje Y positivo) (Num 2)
          *     |  
          *
          *    ____
          *   /    \
          *   |    |    -->  Derecha (Eje X positivo) (Num 0)
          *   |    |         (Direccion de los ejes de las ruedas)
          *   |____|
          *
          * /\ Direccion hacia arriba (Eje Z negativo) (Num 1)
          * \/  (Direccion inversa a la direccion de los resortes de suspension
          *
          **/
         vehiculo->setCoordinateSystem(0, 2, 1);

         const float radioRueda = 0.2;

         const float suspensionStiffness = 20.f;
         const float suspensionDamping = 2.3f;
         const float suspensionCompression = 4.4f;
         const float wheelFriction = 1000;

         const float z = -(0.9*0.2); //El alto del vehiculo menos cierto delta

         agregarRueda( 1 - (0.3*anchoRueda), 1.5 - radioRueda, z, true, radioRueda);
         agregarRueda(-1 + (0.3*anchoRueda), 1.5 - radioRueda, z, true, radioRueda);

         agregarRueda( 1 - (0.3*anchoRueda), -1.5 + radioRueda, z, false, radioRueda);
         agregarRueda(-1 + (0.3*anchoRueda), -1.5 + radioRueda, z, false, radioRueda);


         for (int i = 0; i < vehiculo->getNumWheels(); ++i) {
            btWheelInfo& wheel = vehiculo->getWheelInfo(i);
            wheel.m_suspensionStiffness = suspensionStiffness;
            wheel.m_wheelsDampingRelaxation = suspensionDamping;
            wheel.m_wheelsDampingCompression = suspensionCompression;
            wheel.m_frictionSlip = wheelFriction;
         }


         std::vector<Coord> coordLlanta;
         coordLlanta.push_back(Coord(0, 0, -radioRueda*1.5));
         coordLlanta.push_back(Coord(0, -radioRueda*1.5, 0));
         coordLlanta.push_back(Coord(0, 0, radioRueda*1.5));
         coordLlanta.push_back(Coord(0, radioRueda*1.5, 0));
         
         std::vector<Coord> coordRueda;
         coordRueda.push_back(Coord(-anchoRueda/2.0, 0, 0));
         coordRueda.push_back(Coord(-anchoRueda/4.0, 0, 0));
         coordRueda.push_back(Coord(+anchoRueda/4.0, 0, 0));
         coordRueda.push_back(Coord(+anchoRueda/2.0, 0, 0));
      
         Curva *curvaLlanta = new BsplineCerrada(coordLlanta);
         Curva *curvaRueda = new Bezier(coordRueda);
      
         SuperficieBarrido *rueda = new SuperficieBarrido(*curvaLlanta, *curvaRueda);

         Discretizador disc;
         vertexRueda = disc.superficie(*rueda);
         vertexLlanta = disc.curvaRellena(*curvaLlanta, Coord(-1, 0, 0));

         delete curvaLlanta;
         delete curvaRueda;
         delete rueda;
      }

      void inicializarTexturas(Textura *sup, Textura *lateral) {
         this->sup = sup;
         this->lateral = lateral;
         //inicializar();
      }

      void update() {
         // Si se quiere girar, se gira
         if(debeDoblarIzq) 
            giro += giroInc;

         if(debeDoblarDer) 
            giro -= giroInc;

         // Si no se quiere girar, se "desgasta" el giro
         // acumulado, enderezando el vehiculo
         if(not debeDoblarIzq and not debeDoblarDer) {
            if(giro >= giroInc)
               giro -= giroInc;
            else if(giro <= -giroInc)
               giro += giroInc;
            else
               giro = 0;
         }


         // Limitacion en el giro
         if(giro < -giroMax) giro = -giroMax;
         if(giro > giroMax) giro = giroMax;


         // Si se quiere avanzar, aceleramos (y sacamos el freno)
         if(debeAvanzar) {
            accel += accelInc;
            breakk = 0;
         } 

         // Si se quiere retroceder, vemos:
         //    Si estamos acelerando, aplicamos mas freno
         //    sino, sacamos el freno y seguimos marcha atras
         if(debeRetroceder) {
            accel -= accelInc;
            if(accel > 0) {
               breakk = breakMax;
               accel -= accelInc * 3;
            } else
               breakk = 0;
         }

         // Desgastamos la aceleracion y ponemos el freno
         if(not debeAvanzar and not debeRetroceder) {
            if(accel >= accelInc)
               accel -= accelInc/3.0;
            else if(accel <= -accelInc)
               accel += accelInc/3.0;
            else
               accel = 0;

            if(breakk < breakMax)
               breakk += breakInc;
         }

         // Ponemos un limite
         if(accel > accelMax) accel = accelMax;
         if(accel < -accelMax/2.0) accel = -accelMax/2.0;
         if(breakk > breakMax) breakk = breakMax;

         //Actualizamos...
         //Ruedas delanteras
         vehiculo->setSteeringValue(giro, 0);
         vehiculo->setSteeringValue(giro, 1);

         //Ruedas traseras
         vehiculo->applyEngineForce(accel, 2);
         vehiculo->applyEngineForce(accel, 3);
         vehiculo->setBrake(breakk, 2);
         vehiculo->setBrake(breakk, 3);
      }

      void control(int k, int x, int y, bool activar) {
         switch(k) {
            case GLUT_KEY_LEFT:
               debeDoblarIzq = activar;
               break;
            case GLUT_KEY_RIGHT:
               debeDoblarDer = activar;
               break;
            case GLUT_KEY_UP:
               debeAvanzar = activar;
               break;
            case GLUT_KEY_DOWN:
               debeRetroceder = activar;
               break;
         }
      }

      void dibujar() const {
         float matrix[16];

         for (int i = 0; i < vehiculo->getNumWheels(); ++i) {

            //synchronize the wheels with the (interpolated) chassis worldtransform
            vehiculo->updateWheelTransform(i,true);

            //draw wheels (cylinders)
            vehiculo->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(matrix);
            glPushMatrix();
            glTranslatef(0, 0, 0.2);

            glMultMatrixf(matrix);
            glScalef(2, 2, 2);
            
            glPushMatrix();
            glFrontFace(GL_CW);
            glTranslatef(anchoRueda/2.0, 0, 0);
            vertexLlanta->enlazar();
            vertexLlanta->dibujar();
            glFrontFace(GL_CCW);
            
            glTranslatef(-anchoRueda, 0, 0);
            vertexLlanta->enlazar();
            vertexLlanta->dibujar();
            glPopMatrix();

            glFrontFace(GL_CW);
            vertexRueda->enlazar();
            vertexRueda->dibujar();
            glFrontFace(GL_CCW);

            glPopMatrix();
         }

         glPushMatrix();

         this->transformacion(matrix);
         glMultMatrixf(matrix);

         ejecucion();

         glPopMatrix();
      }

      virtual void ejecucion() const {
         glPushAttrib(GL_ALL_ATTRIB_BITS);
      
         glColor3f(1, 1, 1);
         glMatrixMode(GL_TEXTURE);
         glPushMatrix();
         glLoadIdentity();
         glScalef(-1, 1, 1);
         
         glMatrixMode(GL_MODELVIEW);

         sup->enlazar();
         vertexSup->enlazar();
         vertexSup->dibujar();
         sup->desenlazar();

         lateral->enlazar();
         glPushMatrix();
         glTranslatef(1, 0, 0.5);
         vertexLateral->enlazar();
         vertexLateral->dibujar();
         glPopMatrix();
         glPushMatrix();
         glCullFace(GL_FRONT);
         glTranslatef(-1, 0, 0.5);
         glScalef(-1, 1, 1);
         vertexLateral->enlazar();
         vertexLateral->dibujar();
         glCullFace(GL_BACK);
         glPopMatrix();
         lateral->desenlazar();

         glMatrixMode(GL_TEXTURE);
         glPopMatrix();
         glMatrixMode(GL_MODELVIEW);

         glPopAttrib();
      }

      void posicion(float& x, float &y, float &z, 
            float& dx, float &dy, float &dz) {
         float matrix[16];
         this->transformacion(matrix);

         x = matrix[12];
         y = matrix[13];
         z = matrix[14];

         btVector3 direccion = vehiculo->getForwardVector();
         dx = direccion[0];
         dy = direccion[1];
         dz = direccion[2];
      }

      void direccionArriba(float &ux, float &uy, float &uz) {
         const btTransform& chassisTrans = vehiculo->getChassisWorldTransform(); 
         const int index = vehiculo->getUpAxis();

         ux = chassisTrans.getBasis()[0][index]; 
         uy = chassisTrans.getBasis()[1][index]; 
         uz = chassisTrans.getBasis()[2][index]; 
      }

      ~Vehiculo() {
         delete vehiculo;
         delete vRayCaster;

         delete vertexSup;
         delete vertexLateral;

         delete vertexRueda;
         delete vertexLlanta;
      }

};

#endif
